#include "../Algorithms/followTheLeader.h"
using namespace std;
void leaderQuerry(Network &net, map<int, double> &dist, vector<bool> &inTrain)
{
        int id;
        double d;
        string mess;
        mess = itoa(net.getID());
        mess+= " dfl";
        for(int i = 1; i < net.size(); i++)
        {
                id = -1;
                d = -1.0;
                if(net.isConnected(i) && !inTrain[i])
                {
                        
                        //net.clients[i].writeMessage(mess.c_str(), mess.size());
			net.writeMessage(i, mess);
                        while(1)
                        {
                                net.receiveMessage();
                                if(net.getMS().size() > 0)
                                {
                                        for(int j = 0; j < net.getMS().size(); j++)
                                        {
                                                if(sscanf(net.getMS()[j][0].c_str(), "%i %f", &id, &d) == 2)
                                                {
                                                        if(id == i && d > 0)
                                                        {
                                                                dist[i] = d;
                                                                break;
                                                        }
                                                }
                                        }
                                        if(id == i && d > 0)
                                        {
                                                break;
                                        }
                                }
                        }
                }
                else
                {
                        dist[i] = DBL_MAX;
                }
        }
}



//void leaderDesignate();
void leaderDesignate(Network &net, map<int, double> &dist, vector<bool> &inTrain, int mf)
{
        int id = -1;
        double d = DBL_MAX;
        map<double, int> shortest;
        string mess;
        mess = itoa(net.getID());
        mess += " yal";
        for(int i = 1; i <= dist.size(); i++)
        {
                if(net.isConnected(i) && !inTrain[i])
                {
                        shortest[dist[i]] = i;
                }
        }
        for(map<double, int>::iterator it = shortest.begin(); it != shortest.end(); ++it)
        {
                if(net.writeMessage(it->second, mess) == mess.size())
                {
                        mf = it->second;
                        inTrain[it->second] = true;
                        mess = itoa(it->second);
                        mess += " iaf";
                        net.broadcast(mess);
                        break;
                }
                else
                {
                        //net.isConnected(it->second) = false;
                }
        }
        //mess = itoa(botnum);
        //mess += " tic";
        //net.client.writeMessage(mess.c_str(), mess.size());
}
void leaderWaitForReady(Network& net, int mf)
{
        char cs[50];
        string s;
        int id;
        while(1)
        {
                net.getMS() = net.receiveMessage();
                if(net.getMS().size() > 0)
                {
                        for(int j = 0; j < net.getMS().size(); j++)
                        {
                                if(sscanf(net.getMS()[j][0].c_str(), "%i %s", &id, cs) == 2)
                                {
                                        s = cs;
                                        if(id == mf && s == "iar")//if my follower says I Am Ready
                                        {
                                                return;
                                        }
                                }
                        }
                }
        }
}
void leaderWaitForTrain(Network& net)
{
        char cs[50];
        string s;
        int id;
        while(1)
        {
                net.getMS() = net.receiveMessage();
                if(net.getMS().size() > 0)
                {
                        for(int j = 0; j < net.getMS().size(); j++)
                        {
                                if(sscanf(net.getMS()[j][0].c_str(), "%i %s", &id, cs) == 2)
                                {
                                        s = cs;
                                        if(id == 0 && s == "tic")//if host says Train Is Complete
                                        {
                                                return;
                                        }
                                }
                        }
                }
        }
}


void followerWaitForInstruction(Network& net, int& ml, FindRobots& finder, vector<bool> &inTrain)
{
        char cs[50];
        string mess, s;
        int id;
        std::vector<Robot> robots;
        while(1)
        {
                net.getMS() = net.receiveMessage();
                if(net.getMS().size() > 0)
                {
                        for(int j = 0; j < net.getMS().size(); j++)
                        {
                                if(sscanf(net.getMS()[j][0].c_str(), "%i %s", &id, cs) == 2)
                                {
                                        s = cs;
                                        if(s == "dfl")
                                        {
                                                robots = finder.getRobotPositions();
                                                mess = itoa(net.getID());
                                                if(robots.size() > 0)
                                                {
                                                        mess += ftoa(robots[0].position.radius_cm);
                                                }
                                                else
                                                {
                                                        mess += ftoa(DBL_MAX);
                                                }
                                                net.writeMessage(id, mess);
                                        }
                                        if(s == "yal")
                                        {
                                                inTrain[net.getID()] = true;
                                                ml = id;
                                                break;
                                        }
                                        if(s == "iaf")
                                        {
                                                inTrain[id] = true;
                                        }
                                }
                        }
                }
        }
}
void followerGetReady(Network& net, int& ml, FindRobots& finder, double& rad, double& angle)
{
        double offset = 3.14/16;
        //double rad, angle;
        string mess;
        std::vector<Robot> robots = finder.getRobotPositions();
        Motor motor;
        while(true)
        {

                robots = finder.getRobotPositions();
                //cout << robots.size() << " ";
                rad = -1;
                for(int i = 0; i < robots.size(); i++)
                {
                    if(rad < 0 || robots[i].position.radius_cm < rad)
                    {
                        rad   = robots[i].position.radius_cm;
                        angle = robots[i].position.angle_rad;
                    }
                }
                if(robots.size() > 0)
                {
                    //cout << "Rad " << rad << " Angle " << angle << endl;
                    if(angle > offset)
                    {
                        //cout << "Crawl left\n";
                        motor.leftWheelStop();
                        motor.rightWheelGoOffset(5);
                    }
                    if(angle < -offset)
                    {
                        //cout << "Crawl right\n";
                        motor.rightWheelStop();
                        motor.leftWheelGoOffset(5);
                    }
                    if(angle > -offset && angle < offset)
                    {
                        //cout << "Crawl forward\n";
                        //motor.goOffset(5);
                        mess = itoa(net.getID());
                        mess += " iar";
                        net.writeMessage(ml, mess);
                        break;
                    }

                    struct timespec tim, tim2;
                    tim.tv_sec = 0;
                    tim.tv_nsec = 500000000;

        //            nanosleep(&tim , &tim2);
        //            motor.stop();
        //            nanosleep(&tim , &tim2);
                }
                else {
                    motor.stop();
                }
        }
}
void followerBecomeMiniLeader(Network& net, int& mf, FindRobots& finder, vector<bool> &inTrain, map<int, double>& dist)
{
        string mess;
        bool tic = false;
        tic = true;
        for(int i = 1; i < inTrain.size(); i++)
        {
                if(!inTrain[i] && net.isConnected(i))
                {
                        tic = false;
                        break;
                }
        }
        if(!tic)
        {
                leaderQuerry(net, dist);
                leaderDesignate(net, dist, inTrain, mf);
                leaderWaitForReady(net,  mf);
        }
        else
        {
                mess = itoa(net.getID());
                mess += " tic";
                net.writeMessage(0, mess);
        }
                
}

void followTheLeader(Network& net, bool isLeader, Motor& mr, LED& gn, LED& be, LED& rd, Bumper bump, FindRobots& finder, int color)
{
        vector<bool> inTrain(net.size(), false);
        int ml, mf;
        double rad, angle;
        map<int, double> dist;
        string mess;
        LED l(color);
        bool tic = false;
        if(isLeader)
        {
                inTrain[net.getID()] = true;
                tic = true;
                for(int i = 1; i < inTrain.size(); i++)
                {
                        if(!inTrain[i] && net.isConnected(i))
                        {
                                tic = false;
                                break;
                        }
                }
                if(!tic)
                {
                        leaderQuerry(net, dist);
                        leaderDesignate(net, dist, inTrain, mf);
                        leaderWaitForReady(net,  mf);
                }
                else
                {
                        mess = itoa(net.getID());
                        mess += " tic";
                        net.writeMessage(0, mess);
                }
                l.setLED(false);
                leaderWaitForTrain(net);
                l.setLED(true);
                while(1)
                {
                        net.getMS() = net.receiveMessage();
                        for(int i = 0; i < net.getMS().size(); i++)
                        {       if(net.getMS()[i][0] == "Esc")
                                {
                                        mr.stop();
                                        l.setLED(false);
                                        return;
                                }
                                robotParser(net.getMS()[i][0], mr, gn, be, rd, bump);
                        }
                }

        }
        else
        {
                followerWaitForInstruction(net, ml, finder, inTrain);
                followerGetReady(net, ml, finder, rad, angle);
                followerBecomeMiniLeader(net, mf,finder, inTrain, dist);
                l.setLED(false);
                leaderWaitForTrain(net);
                l.setLED(true);
                while(1)
                {
                        net.getMS() = net.receiveMessage();
                        for(int i = 0; i < net.getMS().size(); i++)
                        {       if(net.getMS()[i][0] == "Esc")
                                {
                                        mr.stop();
                                        l.setLED(false);
                                        return;
                                }
                                //robotParser(net.getMS(), mr, gn, be, rd, bump);
                        }
                        clusterTrack(finder, rad, angle, false);
                }
        }
}
